/*
 * @FileName: agv_handler.cpp
 * @Author: weibin.yin
 * @Date: 2024-9-11 14:26:52
 * @Last Modified by: 
 * @Last Modified time: 
 */
#include "indicators.h"
#include <shared_parameters/shared_params_interface.hpp>

Indicators::~Indicators()
{
  quit_mutex_.unlock();
	thread_.join();
}

bool Indicators::initParam() 
{
    if (!SharedParamsInterface::initialize()) {
        DEBUG_ERROR_OUT("Failed to initialize shared memory interface");
        return false;
    }

    try {
        auto params = SharedParamsInterface::getNodeParams("shared_parameters");
        if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
          DEBUG_ERROR_OUT("Error get shared parameters");
          return false;
        } 

        params = SharedParamsInterface::getNodeParams("indicators");
        for (const auto& [name, value] : params) {
           
            RCLCPP_INFO_STREAM(get_logger(), 
                    "param:" << name << ", value:" << value.toString());
            switch(value.type) {
                case SharedParamsInterface::ParamValue::Type::INT:
                    this->declare_parameter(name, value.getValue<int>());
                    break;
                case SharedParamsInterface::ParamValue::Type::DOUBLE:
                    this->declare_parameter(name, value.getValue<double>());
                    break;
                case SharedParamsInterface::ParamValue::Type::STRING:
                    this->declare_parameter(name, value.getValue<std::string>());
                    break;
                case SharedParamsInterface::ParamValue::Type::BOOL:
                    this->declare_parameter(name, value.getValue<bool>());
                    break;
                default:
                    RCLCPP_WARN_STREAM(get_logger(), 
                            "Unsupported parameter type for " << name);
                    break;
            }
        }
    } catch (const std::exception& e) {
        DEBUG_ERROR_OUT("Error reading parameters: " << e.what());
        return false;
    }
    return true;
}

bool Indicators::pulse_generator(blink_time timeSet, pulse_time &pulse)
{
  bool invert(false);
	TIME_TYPE now_ms= GetTickCount();
	pulse.elapsed_ms = pulse.elapsed_ms + now_ms - pulse.last_ms;  
  int target_time= pulse.level ? timeSet.on_time_ : timeSet.off_time_;

  if ( pulse.elapsed_ms > target_time)
  {
    pulse.elapsed_ms= 0;
    invert= true;
    pulse.level= !pulse.level;
  }
  pulse.last_ms= now_ms;
  if( timeSet.off_time_ == 0) {
    pulse.reset();
  }
	return invert;
}

//TON   The rising edge of the input operand "IN" will start the timer "ON", 
// and it will run as long a time interval as specified by the operand "PT".
bool Indicators::timer_ON(Scene *ptr)
{
  if ( ptr == nullptr)
  {
    return false;
  }
  
  // bool ret(false);
  // TIME_TYPE now_ms= GetTickCount();

  // ptr->speaker_.elapsed_ms_= ptr-> speaker_.level_ ? (ptr->speaker_.elapsed_ms_ + now_ms - ptr->speaker_.last_ms_) : 0;
  // ptr->speaker_.last_ms_= now_ms;
  // return ( ptr->speaker_.elapsed_ms_ > ptr->speaker_.fix_time_.on_time_);
  return true;
}

void Indicators::parserLampCODE(int blink_code, int col, Lamp_s &lamp)
{
  lamp.color_["green"] = TestBit(col, GREEN);
  lamp.color_["yellow"] = TestBit(col, YELLOW);
  lamp.color_["red"] = TestBit(col, RED);  

  if ( blink_code >= 0 && blink_code < blink_times_.size())
  {
    lamp.fix_time_ = blink_times_[blink_code];
    lamp.is_blink_ = blink_code != CONSTANT_ON;
  }
  else
  {
    DEBUG_ERROR_OUT("[parserLampCODE] error blink_code;");
  }
  
}

void Indicators::debug_Callback(const std_msgs::msg::UInt16MultiArray::ConstSharedPtr msg)
{
  if ( msg->data.size() == 1) {
    debugLampScene_= msg->data[0];
  }
  else if ( msg->data.size() == 2) {
    debugLampScene_= msg->data[0];
    debugSpeakerChannel_= msg->data[1];
  }
  else if ( msg->data.size() >= 3) {
    debugLampScene_= msg->data[0];
    debugSpeakerChannel_= msg->data[1];
    debugSpeakerVolume_= msg->data[2];
  }
}

// 读取所有场景下，灯光和音乐设置参数
bool Indicators::readParam()
{ 
    DEBUG_OUT("get indicators param");
    get_parameter_or<bool>("debugLamp", debugLamp_, false);
    get_parameter_or<bool>("debugSpeaker", debugSpeaker_, false);
    debug_sub_ = this->create_subscription<std_msgs::msg::UInt16MultiArray>(
            "/indicators_debug",
            rclcpp::QoS(10),
            std::bind(&Indicators::debug_Callback, this, std::placeholders::_1)
        );	
  
    // 读取参数列表内的输入IO设备
    scenes_.clear();
    std::map<int, std::string> scene_params;
    const auto params = this->list_parameters({}, 10);
   
    for (const auto& param : params.names) {
      size_t prefix_pos = param.find(INDICATORS_NAMESPACE);
      if (prefix_pos == std::string::npos) continue;

      std::string suffix = param.substr(prefix_pos + strlen(INDICATORS_NAMESPACE));
      size_t slash_pos = suffix.find('/');
      if (slash_pos == std::string::npos) continue;

      std::string scene_num_str = suffix.substr(0, slash_pos);
      try {
        int scene_index = std::stoi(scene_num_str);
        scene_params[scene_index] = INDICATORS_NAMESPACE + scene_num_str;
      } catch (const std::exception& e) {
        RCLCPP_WARN(get_logger(), "Invalid scene number in parameter: %s", param.c_str());
      }
    }

    for (const auto& [index, prefix] : scene_params) {
      DEBUG_OUT("###########index:" << index << ",prefix:" << prefix);
      Scene scene;
      scene.index = index;
      scene.name = prefix;

      this->get_parameter(prefix + "/blink_code", scene.lamp_.blink_code_);
      this->get_parameter(prefix + "/color_code", scene.lamp_.color_code_);
      parserLampCODE(scene.lamp_.blink_code_, scene.lamp_.color_code_, scene.lamp_);

      Speaker speaker;
      this->get_parameter(prefix + "/music_channel", speaker.channel);
      this->get_parameter(prefix + "/volume", speaker.volume);
      
      int music_sec = 10, quiet_sec = 20;
      this->get_parameter(prefix + "/music_sec", music_sec);
      this->get_parameter(prefix + "/quiet_sec", quiet_sec);
      
      speaker.fix_time_.on_time_ = std::max(music_sec * 1000, 0);
      speaker.fix_time_.off_time_ = std::max(quiet_sec * 1000, 0);
      scene.speaker_ = speaker;

      scenes_.push_back(scene);

      DEBUG_OUT("scene: " << scene.index << ",blink=" << scene.lamp_.blink_code_ 
            << ",color=" << scene.lamp_.color_code_
            << ",channel=" << scene.speaker_.channel 
            << ",volume=" << scene.speaker_.volume
            << ",music_sec=" << music_sec
            << ",quiet_sec=" << quiet_sec);
    }

    return !scenes_.empty();
}

void Indicators::timerCycleFunc(void)
{
  time_wheel_->tick();
}

void Indicators::statePub( int index, bool gr, bool ylw, bool red, int ch, int vol)
{
  std_msgs::msg::UInt8MultiArray arr;
  arr.data.push_back(index);
  arr.data.push_back(gr);
  arr.data.push_back(ylw);
  arr.data.push_back(red);
  arr.data.push_back(ch);
  arr.data.push_back(vol);

  state_pub_->publish(arr);

  // DEBUG_OUT("[statePub]scene:" << index 
  //           << ";{green:" << gr
  //           << ";yellow:" << ylw
  //           << ";red:"<< red 
  //           << "};{channel:" << ch
  //           << ";volume:"<< vol
  //           << ";}");
}

void Indicators::tw_appendSound( int scene, int delay, std::function<void(int)> func)
{
  auto timer = std::make_shared<gallnut::Timer>( scene, delay, func);
  time_wheel_->append_timer(timer);
}

Scene* Indicators::scene_evaluate(void)
{
  Scene* ptr(nullptr);
  static int index_last(0);
  for (auto &scene : scenes_)
  {
    if ( scene.trigged )  //&& (scene.speaker_.volume != 0)
    {
      //高优先级覆盖低优先级的状态
      ptr = &scene;
    }
  }
    
  if ( debugLamp_ && scenes_.size() > debugLampScene_)
  {
    ptr = &scenes_[debugLampScene_];
  }

  // if ( ptr != nullptr && ptr->speaker_.cycle_ms != 0 )
  // {
  //   ptr->speaker_.insert= timer_ON( ptr);
  // }
  if ( ptr == nullptr)
  {
    ptr = &scenes_[IDLE];
  }

  // 切换场景后,复位控制闪烁的脉冲寄存器
  if ( ptr != nullptr && index_last != ptr->index )
  {
    index_last= ptr->index;
    spk_pulse_.reset();
    lamp_pulse_.reset();
  }
  return ptr;
}
void Indicators::thread_cycle(void)
{
  AUDIO_TYPE audio;
  Speaker spk;
  Lamp_s curlamp;
  Scene* curScene_ptr = scene_evaluate();

  if (curScene_ptr == nullptr)
  {
    DEBUG_ERROR_OUT("[thread_cycle] curScene_ptr is nullptr");  
    return;
  }
  
  //清除输出初始集合
  outputs_.clearOutputs();

  pulse_generator(outputs_time_, outputs_pulse_); 
  //三色灯电平判断
  curlamp = curScene_ptr->lamp_;
  pulse_generator(curlamp.fix_time_, lamp_pulse_);    //脉冲发生器，闪烁效果
  bool green= curlamp.get_level("green", lamp_pulse_.level);     
  bool yellow= curlamp.get_level("yellow", lamp_pulse_.level);   
  bool red= curlamp.get_level("red", lamp_pulse_.level);
  outputs_.setOnePoint(green,  DO_NAME_LAMP_GREEN);
  outputs_.setOnePoint(yellow, DO_NAME_LAMP_YELLOW);
  outputs_.setOnePoint(red,    DO_NAME_LAMP_RED);
  
  //喇叭电平判断
  spk= curScene_ptr->speaker_;
  pulse_generator(spk.fix_time_, spk_pulse_);    //脉冲发生器，使得声音播放一段时间，之后静默一段
  
  audio.first = (spk_pulse_.level && spk.channel != 0) ? spk.channel : 0;  
  audio.second = (spk_pulse_.level && spk.channel != 0) ? spk.volume : 0;   
  outputs_.sound( audio);

  //面板状态指示灯判断      
  bool hostConnect= vehicle_.networkCheck();
  bool lowBat = vehicle_.getTrigs().low_battery || vehicle_.getTrigs().too_low_battery;
  bool location = vehicle_.poseCheck();
  bool emg = vehicle_.getTrigs().estop_button || vehicle_.getTrigs().collision_sensor;
  bool reset = vehicle_.alarmCheck();
  outputs_.setOnePoint( hostConnect, DO_NAME_LAMP_HOST, outputs_pulse_.level);
  outputs_.setOnePoint( lowBat, DO_NAME_LAMP_LOWBAT, outputs_pulse_.level);
  outputs_.setOnePoint( location, DO_NAME_LAMP_LOCATION, outputs_pulse_.level);
  outputs_.setOnePoint( emg, DO_NAME_LAMP_EMGECY, outputs_pulse_.level);
  outputs_.setOnePoint( reset, DO_NAME_LAMP_RESET, outputs_pulse_.level);

  //特殊io逻辑;例如叉车探照灯
  bool driving= scenes_[DRIVING].trigGet();
  bool left= scenes_[TURN_LEFT].trigGet();
  bool right= scenes_[TURN_RIGHT].trigGet();
  outputs_.setOnePoint( driving, DO_NAME_OUTLIEN_LAMP, outputs_pulse_.level);
  outputs_.setOnePoint( true, DO_NAME_WINDFAN, outputs_pulse_.level);
  outputs_.setOnePoint( true, DO_NAME_THIRDLIFT, outputs_pulse_.level);
  outputs_.setOnePoint( left, DO_NAME_LEFTTURN, outputs_pulse_.level);
  outputs_.setOnePoint( right, DO_NAME_RIGHTTURN, outputs_pulse_.level);

  // 汇总完状态，最终调用IO输出接口
  bool ret = outputs_.outputsPub();
  // DEBUG_OUT("[thread_cycle]scene:" << curScene_ptr-> index 
  //             << ";level_:" << outputs_pulse_.level
  //             << ";");
  if (ret)
  {
    DEBUG_OUT("[thread_cycle]scene:" << curScene_ptr-> index 
              << ";level_:" << lamp_pulse_.level
              << ";on:" << curlamp.fix_time_.on_time_
              << ";off:" << curlamp.fix_time_.off_time_
              << ";{green:" << green
              << ";yellow:" << yellow
              << ";red:"<< red 
              << "};{channel:" << audio.first
              << ";volume:"<< audio.second
              << ";on:" << spk.fix_time_.on_time_
              << ";off:" << spk.fix_time_.off_time_                          
              << ";} {netConnect:" << vehicle_.networkCheck()
              << ";low_battery:" << vehicle_.getTrigs().low_battery
              << ";poseCheck:" << vehicle_.poseCheck()
              << ";estop_button:" << emg
              << ";}");
  }
  statePub(curScene_ptr->index, green, yellow, red, audio.first, audio.second);
}

void Indicators::thread_fun(Indicators *pThis)
{
  if (!pThis)
    return;

  while(!pThis->quit_mutex_.try_lock_for(std::chrono::milliseconds( 20))){
    pThis->thread_cycle();
  }
}

/*!
 * \brief Initializes node to get parameters, subscribe to topics.
 */
void Indicators::init()
{
  bool ret = readParam();
  if (ret)
  {
    state_pub_ =  this->create_publisher<std_msgs::msg::UInt8MultiArray>(
            "/indicators_state",
            rclcpp::QoS(10).reliable());

    bool enable_speaker = SharedParamsInterface::getParam("IO", "canSpeaker/enable", true);
    vehicle_.init(shared_from_this());
    outputs_.init(shared_from_this(), enable_speaker);
    quit_mutex_.lock();
    thread_ = std::thread(thread_fun, this);
    DEBUG_OUT("[Indicators] init success,create scene thread and warning system");
  }
  else
  {
    DEBUG_ERROR_OUT("[Indicators] init failed, can't find scenes param");
  }
  
  // timer_ = n_.createTimer(rclcpp::Duration( 1.0), boost::bind(&Indicators::timerCycleFunc, this)); 
  
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Indicators>();
  node->init();
  rclcpp::executors::MultiThreadedExecutor executor(
      rclcpp::ExecutorOptions(), 
      8  // 线程数
  );
  
  executor.add_node(node);
  executor.spin();
	rclcpp::shutdown();
  return(0);
}
